using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.AI;


[RequireComponent(typeof(NavMeshAgent))]
public class NavMove : MonoBehaviour
{
    // private List<Vector3> pastFrame;
    private NavMeshAgent nav;
    public Transform target;
    public List<Matrix4x4> recPos;
    public bool isCompleted;
    private float gap = float.PositiveInfinity;

    public void ResetParams()
    {
        recPos = new List<Matrix4x4>();
        isCompleted = false;
        target = null;
        gap = float.PositiveInfinity;        
    }

    public void SetNav() {
        nav = this.GetComponent<NavMeshAgent>();
    }

    public void ClearNav()
    {
        this.nav = null;
    }

    // Start is called before the first frame update
    void Start()
    {
        nav = this.GetComponent<NavMeshAgent>();
        ResetParams();
    }

    // Update is called once per frame
    void Update()
    {
        if (nav != null && target != null && !isCompleted)
        {
            Vector3 tarPos = target.GetWorldMatrix().GetPosition();
            // Note the following to be used: world coordinate calculation
            if (recPos.Count > 30)
            {              
                float dist = 0f;
                for (int i = 0; i < 10; i++)
                {
                    int idx = recPos.Count - 10 + i;
                    dist = dist + (recPos[idx].GetPosition() - tarPos).sqrMagnitude;
                }

                // float diff = (transform.position - target.position).sqrMagnitude;   
                float diff = (transform.GetWorldMatrix().GetPosition() - tarPos).sqrMagnitude;      

                // Debug.Log(gap);
                // Debug.Log(diff);         
                // Debug.Log(Mathf.Abs(dist / 10 - diff));
                // Debug.Log(transform.position);
                // Debug.Log(transform.GetWorldMatrix().GetPosition());
                // Debug.Log(target.position);
                // Debug.Log(tarPos);

                if (Mathf.Abs(dist / 10 - diff) < 0.001f && gap <= diff)
                {
                    isCompleted = true;
                    target = null;
                    return;
                }

                if(diff < gap){
                    gap = diff;
                }
            }
            nav.destination = tarPos;
            recPos.Add(transform.GetWorldMatrix());
            // Debug.Log(transform.position);
        }
    }
}
